% plot the robotic model position

% plot the robotic model with the angles from the human mean data
% at the beginning of a step
% s_ankle = -0.5171
% s_knee =  0.3771
% theta3 = 0.1186
% theta4 = -0.2076
% ns_knee = 0.1376
opt_addpath();
% x_opt = [s_ankle(1);s_knee(1);theta3(1);theta4(1)+0.049;ns_knee(1);0;0;0;0;0];
x_opt = [s_ankle(1);s_knee(1);theta3(1);theta4(1);ns_knee(1);0;0;0;0;0];
% x_opt= [ .3271;0;0;0;0;0;0;0;0;0]
pos = jpos_mat(x_opt)

%     pos = jpos_mat(x)
 legs = plot([0 0], 'r');  
%  axis([-.2 .2 -0.1 1.2]);%
%  axis equal
         set(legs, 'XData', pos(1,:)*1000+987.4, 'YData', pos(2,:)*1000-856.3, ...
            'erasemode', 'normal');
        hold on;
        
%            plot(pos(1,4)*1000+987.4,pos(2,4)*1000-856.3,'ro');
%    hold on;
%    plot(pos(1,7)*1000+987.4,pos(2,7)*1000-856.3,'go');
   
%%   
   hold on;
% x_opt = [s_ankle(end);s_knee(end);theta3(end);theta4(end)-0.2333;ns_knee(end);0;0;0;0;0];
x_opt = [s_ankle(end);s_knee(end);theta3(end);theta4(end);ns_knee(end);0;0;0;0;0];
pos = jpos_mat(x_opt)

%     pos = jpos_mat(x)
 legs = plot([0 0], 'r');  
%  axis([-.2 .2 -0.1 1.2]);%
%  axis equal
         set(legs, 'XData', pos(1,:)*1000+1043, 'YData', pos(2,:)*1000-869.3, ...
            'erasemode', 'normal');
        hold on;
        
%            plot(pos(1,4),pos(2,4),'ro');
%    hold on;
%    plot(pos(1,7),pos(2,7),'go');
   
   StanceLeg2DLeft(Selina,537)
   %% plot human data
   addpath('/home/shu/Documents/Research_Related/human_data_analysis')
   load('/home/shu/Documents/Research_Related/human_data_analysis/Selina/Selina_Data.mat')
%    263 537
   StanceLeg2DLeft(Selina,263)